﻿/*************************************************
  Copyright (C), 2014-2017, lmd Tech. Co., Ltd.
  文 件  名:	Assets\Scripts\Tools\MoveUtil\MoveUtil.cs
  作       者:	陈佳
  版       本:	1.0        
  完成日期:	2017//      
  功能描述:	移动相关的工具
  主要功能: 变速后匀速位移计算;
				多段变速位移计算;
*************************************************/
//#define DEBUG_MATHUTIL
//#define DEBUG_CORRECT_POSITION

using System;
using System.Collections.Generic;
using CxExtension;
using UnityEngine;

namespace CxExtension
{
	/// <summary>
	/// 加速度信息的结构体
	/// </summary>
	[Serializable]
	public struct SAccelerData
	{
		public float minVelocity;               //最小速度
		public float targetVelocity;            //目标速度
		public float a;                             //加速度
	}
	/// <summary>
	/// 移动相关的工具类,封装了多段加速,加速后匀速等工具函数
	/// </summary>
	public class MoveUtil
	{
		public struct SMoveResult
		{
			public float s;                             //位移量
			public float v;                             //速度
			public float usedtime;                  //加速阶段使用时间
			public float remaintime;                //剩余没有计算位移的时间
			public override string ToString()
			{
				base.ToString();
				var str = string.Format("use:{2}s to {1} run:{0},remain:{3}s,", s, v, usedtime, remaintime);
				return str;
			}
		}

		/// <summary>
		/// 多段加速函数,根据速度匹配某个加速度的匹配结果
		/// </summary>
		public struct SMatchResult
		{
			public bool hasValue;                   //是否有值的标记
			public SAccelerData value;          //值的结构体
			public SMatchResult(SAccelerData value)
			{
				hasValue = true;
				this.value = value;
			}
			public SMatchResult(bool hasValue)
			{
				this.hasValue = hasValue;
				this.value = new SAccelerData();
			}
			public static SMatchResult Null
			{
				get
				{
					return new SMatchResult(false);
				}
			}
		}

		/// <summary>
		/// 以a每秒的加速度,从v0加速到v1
		/// </summary>
		/// <param name="v0">起始速度</param>
		/// <param name="v1">目标速度</param>
		/// <param name="a">加速度</param>
		/// <param name="t">时间</param>
		/// <param name="result">计算的结果</param>
		/// <returns>给定时间是否有加速</returns>
		public static bool Jiasu(float v0, float v1, float a, float t, out SMoveResult result)
		{
#if DEBUG_MATHUTIL
			Debuger.LogFormat("MathUtil.Jiasu:开始从 {0} 到 {1},以{2}每秒加速度,运行{3}秒",v0,v1,a,t);
#endif
			if(a <= 0)
			{
				//Debuger.ErrFormat("MathUtil.Move参数a:{0} 不能小于0",a);
				result.v = v0;
				result.s = v0 * t;
				result.usedtime = t;
				result.remaintime = 0;
				return false;
			}

			var s = 0f;

			MathUtil.SLerpResult lResult;
			var changed = MathUtil.Lerp(v0, v1, a, t, out lResult);

			//如果有速度改变说明包含变速阶段
			if(changed)
			{
				//加速时间
				var at = lResult.usedtime;
				//区分加速,减速
				a *= lResult.speedDir;
				s += v0 * at + 0.5f * a * at * at;
			}

			result.v = lResult.v;
			result.s = s;
			result.usedtime = lResult.usedtime;
			result.remaintime = lResult.remaintime;
#if DEBUG_MATHUTIL
			Debuger.LogFormat("MathUtil.Jiasu:结束:从 {0} 到 {1},以{2}每秒加速度,运行{3}秒,移动{4}米,剩下:{5}秒",v0,v1,a,result.usedtime,result.s,result.remaintime);
#endif
			return changed;
		}

		/// <summary>
		/// 加速到目标值后匀速
		/// </summary>
		/// <param name="v0">初始速度</param>
		/// <param name="v1">目标速度</param>
		/// <param name="a">加速度</param>
		/// <param name="t"></param>
		/// <param name="result"></param>
		/// <returns></returns>
		public static bool JiasuAndYunsu(float v0, float v1, float a, float t, out SMoveResult result)
		{
			SMoveResult r;
			Jiasu(v0, v1, a, t, out r);
			var hasAccelerate = false;
			if(r.remaintime > 0)
			{
				hasAccelerate = true;

				r.s += v1 * r.remaintime;
#if DEBUG_MATHUTIL
				Debuger.LogFormat("MathUtil.JiasuAndYunsu:匀速:{0}运行{1}秒,移动{2}米",v1,r.remaintime,v1 * r.remaintime);
#endif
			}

			result.v = r.v;
			result.s = r.s;
			result.usedtime = t;
			result.remaintime = 0;
#if DEBUG_MATHUTIL
			Debuger.LogFormat("MathUtil.JiasuAndYunsu:indata:v0={0},v1={1},a={2},t={3}",v0,v1,a,t);
			Debuger.LogFormat("MathUtil.JiasuAndYunsu:outdata={0}",result);
#endif
			return hasAccelerate;
		}

		/// <summary>
		/// 多段变加速,迭代器中的数据需要时有序的,加速时速度递增,减速时速度递减,不然无法匹配合适的加速度,需要人为设置是加速还是减速
		/// </summary>
		/// <param name="accelers">加速迭代器</param>
		/// <param name="v0">初始速度</param>
		/// <param name="dt">时间</param>
		/// <param name="isAccelerate">true是加速,false是减速</param>
		/// <param name="result"></param>
		/// <returns>如果速度有改变 返回 true</returns>
		public static bool MultistageAccelerate(IEnumerator<SAccelerData> accelers, float v0, float dt, bool isAccelerate, out SMoveResult result)
		{
			float v1 = 0, a = 0, s = 0, t = dt;
			var hasTarget = false;
			var hasChange = false;
#if DEBUG_MATHUTIL
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:from {0} to {1} ,a={2},t={3}",v0,v1,a,t);
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:outdata={0}",result);
#endif
			while(t > 0)
			{
				//查找下一个加速阶段
				hasTarget = accelers.MatchPos((it) =>
				 {
					 if(isAccelerate)
					 {
						 return v0 < it.targetVelocity;
					 }
					 else
					 {
						 return v0 > it.targetVelocity;
					 }
				 });
				//如果有阶段,加速
				if(hasTarget)
				{
					var ainfo = accelers.Current;
					SMoveResult r;
					v1 = ainfo.targetVelocity;
					a = ainfo.a;

					Jiasu(v0, v1, a, t, out r);
					//Debug.LogFormat("匹配加速度,速度:{0}")
					//迭代剩余时间,重设初始速度,积累位移量
					t = r.remaintime;
					v0 = r.v;
					s += r.s;
					hasChange = true;
				}
				//没有就匀速
				else
				{
					a = 0;
					s += v0 * t;
#if DEBUG_MATHUTIL
					Debuger.LogFormat("MathUtil.MultistageAccelerate:匀速:{0}运行{1}秒,移动{2}米",v0,t,v0 * t);
#endif
					break;
				}
			}
			result.s = s;
			result.v = v0;
			result.usedtime = dt;
			result.remaintime = 0;
#if DEBUG_MATHUTIL
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:indata:v0={0},v1={1},a={2},t={3}",v0,v1,a,t);
			Debuger.LogFormat("MathUtil.MultistageAccelerate:outdata={0}",result);
#endif
			return hasChange;
		}

		public static bool MultistageAccelerate(Func<float, SMatchResult> matchFunc, float v0, float dt, out SMoveResult result)
		{
			float v1 = 0, a = 0, s = 0, t = dt;
			var hasTarget = false;
			var hasChange = false;
#if DEBUG_MATHUTIL
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:from {0} to {1} ,a={2},t={3}",v0,v1,a,t);
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:outdata={0}",result);
#endif
			while(t > 0)
			{
				//查找下一个加速阶段
				var nextv = matchFunc(v0);
				hasTarget = nextv.hasValue;
				//如果有阶段,加速
				if(hasTarget)
				{
					var ainfo = nextv.value;
					SMoveResult r;
					v1 = ainfo.targetVelocity;
					a = ainfo.a;

					Jiasu(v0, v1, a, t, out r);
					//Debug.LogFormat("匹配加速度,速度:{0}")
					//迭代剩余时间,重设初始速度,积累位移量
					t = r.remaintime;
					v0 = r.v;
					s += r.s;
					hasChange = true;
				}
				//没有就匀速
				else
				{
					a = 0;
					s += v0 * t;
#if DEBUG_MATHUTIL
					Debuger.LogFormat("MathUtil.MultistageAccelerate:匀速:{0}运行{1}秒,移动{2}米",v0,t,v0 * t);
#endif
					break;
				}
			}
			result.s = s;
			result.v = v0;
			result.usedtime = dt;
			result.remaintime = 0;
#if DEBUG_MATHUTIL
			//Debuger.LogFormat("MathUtil.MultistageAccelerate:indata:v0={0},v1={1},a={2},t={3}",v0,v1,a,t);
			Debuger.LogFormat("MathUtil.MultistageAccelerate:outdata={0}",result);
#endif
			return hasChange;
		}
		/// <summary>
		/// 根据1和2当前的距离,速度,加速度,计算出,t秒后他们之间的距离
		/// 注意:加速度区分正负,正表示加速 负表示减速
		/// 1                             2                      1`               2`
		/// |______________________|________________|____________|
		/// |     nowDistance      |                       |       s        |
		/// </summary>
		/// <param name="nowDistance">两者现在的距离</param>
		/// <param name="u1">1的速度</param>
		/// <param name="a1">1的加速度</param>
		/// <param name="u2">2的速度</param>
		/// <param name="a2">2的加速度</param>
		/// <param name="t">预算的时间</param>
		/// <returns>预算后他们的距离,如果为正,12的先后不改变,如果为负,表示1操作2,在2的前面</returns>
		public static float DistanceOnAftertime(float nowDistance, float u1, float a1, float u2, float a2, float t)
		{
			if(t <= 0)
			{
				Debuger.ErrFormat("时间不能为{0}", t);
				return nowDistance;
			}
			var s1 = u1 * t + 0.5f * a1 * t * t;
			var s2 = u2 * t + 0.5f * a2 * t * t;
			var s = s2 + nowDistance - s1;
			return s;
		}
		public static float DistanceOnAftertime(float nowDistance, float u1, float a1, float s2, float t)
		{
			if(t <= 0)
			{
				Debuger.ErrFormat("时间不能为{0}", t);
				return nowDistance;
			}
			var s1 = u1 * t + 0.5f * a1 * t * t;
			//var s2 = u2 * t + 0.5f * a2 * t * t;
			var s = s2 + nowDistance - s1;
			return s;
		}

		/// <summary>  
		/// 根据1和2当前的距离,速度,加速度,计算出,多少秒后他们之间的距离等于s
		/// 注意:加速度区分正负,正表示加速 负表示减速
		/// 1                             2                      1`               2`
		/// |______________________|________________|____________|
		/// |     nowDistance      |                       |       s        |
		/// u1 * t + 0.5f * a1 * t * t + s = u2 * t + 0.5 * a2 * t * t + nowDistance
		/// a = 0.5f * ( a1 - a2);
		/// b = u1 - u2;
		/// c = s - nowDistance;
		/// </summary>
		/// <param name="nowDistance"></param>	
		/// <param name="u1">1的速度</param>
		/// <param name="a1">1的加速度</param>
		/// <param name="u2">2的速度</param>
		/// <param name="a2">2的加速度</param>
		/// <param name="s">误差距离</param>
		/// <returns>多少秒后可以相遇</returns>
		public static float Meettime(float nowDistance, float u1, float a1, float u2, float a2, float s)
		{
			var a = 0.5f * (a1 - a2);
			var b = u1 - u2;
			var c = s - nowDistance;
			float bigNum = 10000;
			if(MathUtil.Equal(a, 0f))
			{
				Debug.LogErrorFormat("两者加速度{0}和{1}非常相近,相遇时间无穷大", a1, a2);
				return bigNum;
			}
			var derta = b * b - 4 * a * c;
			if(derta < 0)
			{
				Debug.LogErrorFormat("此方程无解,nowdistance:{0} ,u1:{1} ,a1:{2} ,u2:{3} ,a2:{4} ,s:{5}", nowDistance, u1, a1, u2, a2, s);
				return bigNum;
			}
			var sqr = Mathf.Sqrt(derta);
			var fenmu1 = -b + sqr;
			var fenmu2 = -b - sqr;
			var fenzi = 2 * a;
			var t1 = fenmu1 / fenzi;
			var t2 = fenmu2 / fenzi;
			//如果两个解都大于零,取最小的解
			if(t1 > 0 && t2 > 0)
			{
				return t1 < t2 ? t1 : t2;
			}
			//如果两个解都小于零,没有解
			else if(t1 < 0 && t2 < 0)
			{
				return bigNum;
			}
			//如果两个解有一个大于零 取大于零的解
			else
			{
				return t1 > t2 ? t1 : t2;
			}
		}
		public static float MatchAcceleration(List<SAccelerData> accelerations, float curVelocity)
		{
			SAccelerData result;
			var exist = accelerations.FindA(it => MathUtil.IsRound(curVelocity, it.minVelocity, it.targetVelocity), out result);
			return exist ? result.a : 0;
		}
	}
}
